/*
 * IR.java
 *
 * Created on 10 de noviembre de 2007, 10:37 PM
 *
 * To change this template, choose Tools | Template Manager
 * and open the template in the editor.
 */

package Robot.Sensors;

import Robot.*;
import javaclient3.*;
import java.util.ArrayList;
import javaclient3.structures.PlayerConstants;
import javaclient3.structures.ir.PlayerIrPose;
import javaclient3.structures.PlayerPose3d;


/**
 *
 * @author alejo
 */
public class IR extends Sensor implements FloatArraySensor{
    
    IRInterface IRDev;
    int numIRs;
    float [] data;
    //angles of each 
    double [] poseAngles;
    boolean isGeomQueried = false;
    
    /** Creates a new instance of IR */
    public IR(Robot robot, String name, int index) throws Exception{
        
        super(robot, name, index);
        
        IRDev=robot.requestInterfaceIR(deviceIndex, PlayerConstants.PLAYER_OPEN_MODE);
        
        if(IRDev!=null)
            System.out.println("interface IR:"+Integer.toString(deviceIndex)+" of Robot:"+
                                        Integer.toString(robot.getIndex())+" Ok");
        else{
            System.out.println("interface IR "+Integer.toString(deviceIndex)+" of Robot:"+
                                        Integer.toString(robot.getIndex())+" Fail");
            return;
        }
        
        this.setPlayerDevice(IRDev);
        
        

    }
    
     public float[] read(int mode) {
        
        //if READ_MODE_WAIT it will waits until new data is available
        //else, returns the old data
        switch(mode)
        {
            case READ_MODE_WAIT:
                while(!update());
            break;
            case READ_MODE_NO_WAIT:
                update();
            break;
        
        }
        return data; 
    }
     
    public boolean update(){
        
        if(isReady()) {
            data = IRDev.getData().getRanges();
            
            return true;
        }
        else{
            return false;
        }
    }
    
    public boolean isReady()
    {
        return IRDev.isDataReady();
    }
    
    
    public double [] getPoses()
    {
        return poseAngles;
    }

    public boolean updatePoses(){
        
        
        
        if(!isGeomQueried){
            IRDev.queryPose();
            isGeomQueried = true;
        }
        
        PlayerPose3d [] posesTemp;
            
        

        //wait until geometry data is obtained from server
        if(IRDev.isPoseReady())
        {

            numIRs = IRDev.getPose().getPoses_count();
            posesTemp = IRDev.getPose().getPoses();
            

            System.out.println("Number of Poses for sonar "+Integer.toString(deviceIndex)+": "+ Integer.toString(numIRs));

            poseAngles = new double[numIRs];
            data = new float[numIRs];


            //get angle of poses
            for(int i=0;i<numIRs;i++){ 
                poseAngles[i]=posesTemp[i].getPyaw();
                System.out.println("x: "+Double.toString(posesTemp[i].getPx())+" y: "+Double.toString(posesTemp[i].getPy())+" yaw: "+Double.toString(Math.toDegrees(posesTemp[i].getPyaw())));
               // System.out.println(Float.toString(poseAngles[i]));
            }

            
            return true;
        }
        else
            return false;

    
    }
    
}
